orientation.h

Go to the documentation of this file.

// Copyright (c) 2014, 2015, 2016, NXP Semiconductors N.V.,
// All rights reserved.
// 
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
//     * Redistributions of source code must retain the above copyright
//       notice, this list of conditions and the following disclaimer.
//     * Redistributions in binary form must reproduce the above copyright
//       notice, this list of conditions and the following disclaimer in the
//       documentation and/or other materials provided with the distribution.
//     * Neither the name of NXP Semiconductors N.V. nor the
//       names of its contributors may be used to endorse or promote products
//       derived from this software without specific prior written permission.
// 
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
// DISCLAIMED. IN NO EVENT SHALL NXP SEMICONDUCTORS N.V. BE LIABLE FOR ANY
// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

/*! \file orientation.h
    \brief Functions to convert between various orientation representations

    Functions to convert between various orientation representations.  Also
    includes functions for manipulating quaternions.
*/
 
#ifndef ORIENTATION_H
#define ORIENTATION_H

/// quaternion structure definition
typedef struct Quaternion       
{
    float q0;           ///< scalar component
    float q1;           ///< x vector component
    float q2;           ///< y vector component
    float q3;           ///< z vector component
} Quaternion;
              
// function prototypes
/// Aerospace NED accelerometer 3DOF tilt function, computing rotation matrix fR
void f3DOFTiltNED(
    float fR[][3],              ///< computed rotation matrix (output)
    float fGc[]                 ///< calibrated accelerometer input vector
);
/// Android accelerometer 3DOF tilt function computing, rotation matrix fR
void f3DOFTiltAndroid(
    float fR[][3],              ///< computed rotation matrix (output)
    float fGc[]                 ///< calibrated accelerometer input vector
);
/// Windows 8 accelerometer 3DOF tilt function computing, rotation matrix fR
void f3DOFTiltWin8(
    float fR[][3],              ///< computed rotation matrix (output)
    float fGc[]                 ///< calibrated accelerometer input vector
);
/// Aerospace NED magnetometer 3DOF flat eCompass function, computing rotation matrix fR
void f3DOFMagnetometerMatrixNED(
    float fR[][3],              ///< computed rotation matrix (output)
    float fBc[]                 ///< calibrated magnetometer reading (input)
);
/// Android magnetometer 3DOF flat eCompass function, computing rotation matrix fR
void f3DOFMagnetometerMatrixAndroid(
    float fR[][3],              ///< computed rotation matrix (output)
    float fBc[]                 ///< calibrated magnetometer reading (input)
);
/// Windows 8 magnetometer 3DOF flat eCompass function, computing rotation matrix fR
void f3DOFMagnetometerMatrixWin8(
    float fR[][3],              ///< computed rotation matrix (output)
    float fBc[]                 ///< calibrated magnetometer reading (input)
);
/// NED: basic 6DOF e-Compass function, computing rotation matrix fR and magnetic inclination angle fDelta
void feCompassNED(
    float fR[][3],              ///< computed rotation matrix (output)
    float *pfDelta,             ///< magnetic inclination angle (output)
    float *pfsinDelta,          ///< sin of the inclination angle
    float *pfcosDelta,          ///< cos of the inclination angle
    float fBc[],                ///< calibrated magnetometer vector (input)
    float fGc[],                ///< calibrated accelerometer input vector (input)
    float *pfmodBc,             ///< modulus of the calibrated magnetic vector
    float *pfmodGc              ///< modulus of the calibrated accelerometer vector
);
/// Android: basic 6DOF e-Compass function, computing rotation matrix fR and magnetic inclination angle fDelta
void feCompassAndroid(
    float fR[][3],              ///< computed rotation matrix (output)
    float *pfDelta,             ///< magnetic inclination angle (output)
    float *pfsinDelta,          ///< sin of the inclination angle
    float *pfcosDelta,          ///< cos of the inclination angle
    float fBc[],                ///< calibrated magnetometer reading (input)
    float fGc[],                ///< calibrated accelerometer input vector (input) 
    float *pfmodBc,             ///< modulus of the calibrated magnetic vector
    float *pfmodGc              ///< modulus of the calibrated accelerometer vector
);
/// Win8: basic 6DOF e-Compass function, computing rotation matrix fR and magnetic inclination angle fDelta
void feCompassWin8(
    float fR[][3],              ///< computed rotation matrix (output)
    float *pfDelta,             ///< magnetic inclination angle (output)
    float *pfsinDelta,          ///< sin of the inclination angle
    float *pfcosDelta,          ///< cos of the inclination angle
    float fBc[],                ///< calibrated magnetometer reading (input)
    float fGc[],                ///< calibrated accelerometer input vector (input) 
    float *pfmodBc,             ///< modulus of the calibrated magnetic vector
    float *pfmodGc              ///< modulus of the calibrated accelerometer vector
);
/// extract the NED angles in degrees from the NED rotation matrix
void fNEDAnglesDegFromRotationMatrix(
    float R[][3],               ///< rotation matrix input
    float *pfPhiDeg,            ///< output: the roll angle range -180.0 <= Phi < 180.0 deg
    float *pfTheDeg,            ///< output: the pitch angle -90.0 <= Theta <= 90.0 deg
    float *pfPsiDeg,            ///< output: the yaw (compass) angle 0.0 <= Psi < 360.0 deg
    float *pfRhoDeg,            ///< output: For NED, the compass heading Rho equals the yaw angle Psi
    float *pfChiDeg             ///< output: the tilt angle from vertical Chi (0 <= Chi <= 180 deg)
);
/// extract the Android angles in degrees from the Android rotation matrix
void fAndroidAnglesDegFromRotationMatrix(
    float R[][3],               ///< rotation matrix input
    float *pfPhiDeg,            ///< the roll angle -90.0 <= Phi <= 90.0 deg
    float *pfTheDeg,            ///< the pitch angle -180.0 <= The < 180.0 deg
    float *pfPsiDeg,            ///< yaw angle Psi with range 0.0 <= Psi < 360.0 deg
    float *pfRhoDeg,            ///< the compass heading angle Rho equals the yaw angle Psi
    float *pfChiDeg             ///< the tilt angle from vertical Chi (0 <= Chi <= 180 deg)
);
/// extract the Windows 8 angles in degrees from the Windows 8 rotation matrix
void fWin8AnglesDegFromRotationMatrix(
    float R[][3],               ///< rotation matrix input
    float *pfPhiDeg,            ///< the roll angle -90.0 <= Phi <= 90.0 deg
    float *pfTheDeg,            ///< pitch angle Theta in the range -180.0 <= The < 180.0 deg
    float *pfPsiDeg,            ///< yaw angle Psi in range 0.0 <= Psi < 360.0 deg
    float *pfRhoDeg,            ///< the compass angle Rho = 360 - Psi
    float *pfChiDeg             ///< tilt angle from vertical Chi (0 <= Chi <= 180 deg)
);
/// compute the orientation quaternion from a 3x3 rotation matrix
void fQuaternionFromRotationMatrix(
    float R[][3],               ///< Rotation matrix (input)
    Quaternion *pq              ///< Quaternion (output)
);
/// compute the rotation matrix from an orientation quaternion
void fRotationMatrixFromQuaternion(
    float R[][3],               ///< Rotation matrix (output)
    const Quaternion *pq        ///< Quaternion (input)
);
/// function compute the quaternion product qB * qC
void qAeqBxC(
    Quaternion *pqA, 
    const Quaternion *pqB, 
    const Quaternion *pqC
);
/// function compute the quaternion product qA = qA * qB
void qAeqAxB(
    Quaternion *pqA, 
    const Quaternion *pqB
);
/// function compute the quaternion product conjg(qA) * qB
Quaternion qconjgAxB(
    const Quaternion *pqA,
    const Quaternion *pqB
);
/// function normalizes a rotation quaternion and ensures q0 is non-negative
void fqAeqNormqA(
    Quaternion *pqA
);
/// set a quaternion to the unit quaternion
void fqAeq1(
    Quaternion *pqA
);
/// computes normalized rotation quaternion from a rotation vector (deg)
void fQuaternionFromRotationVectorDeg(
    Quaternion *pq,             ///< quaternion (output)
    const float rvecdeg[],      ///< rotation vector in degrees
    float fscaling              ///< delta Time
);
/// computes rotation vector (deg) from rotation quaternion
void fRotationVectorDegFromQuaternion(
    Quaternion *pq,             ///< quaternion (input)
    float rvecdeg[]             ///< rotation vector in degrees (output)
);
/// function low pass filters an orientation quaternion and computes virtual gyro rotation rate
void fLPFOrientationQuaternion(
    Quaternion *pq, 
    Quaternion *pLPq, 
    float flpf, 
    float fdeltat, 
    float fOmega[]
);
/// function computes the rotation quaternion that rotates unit vector u onto unit vector v as v=q*.u.q
/// using q = 1/sqrt(2) * {sqrt(1 + u.v) - u x v / sqrt(1 + u.v)}
void fveqconjgquq(
  Quaternion *pfq, 
  float fu[], 
  float fv[]
);

#endif   // #ifndef ORIENTATION_H